from machine import Pin, I2C
import math 
Ki=0.005
Kp=1.5
halfT=0.005
s1=0;s2=0;s3=0
matrix=[]
clk = Pin(23, Pin.OUT)   # Select the 29 pin device as the clock
sda = Pin(22, Pin.IN)   # Select the 30 pin device as the data line
i2c = I2C(-1,clk, sda, freq=1000000) # create I2C peripheral at frequency of 100kH
j1=[];j2=[];j3=[];j4=[];j5=[];j6=[]
u=[]
NormAccz=0
RC_PIT=0.0
RC_ROL=0.0
RC_YAW=0.0
Butter_20HZ_Parameter_Acce = [
	[-0.00777631271910257, 0.06445464557871, 0.443321667140393],
	[0.443321667140393, 0.06445464557871, -0.00777631271910257]]
RC_THROTTLE = 0
YM_Dead=100
Input_Butter=[]
Output_Butter=[]
def init():
    i2c.writeto_mem(0x68, 0x6B, b'\x01')
    i2c.writeto_mem(0x68, 0x6C, b'\x00')
    i2c.writeto_mem(0x68, 0x19, b'\x05')
    i2c.writeto_mem(0x68, 0x1A, b'\x07')
    i2c.writeto_mem(0x68, 0x1B, b'\x18')
    i2c.writeto_mem(0x68, 0x1C, b'\x18')
init()
def zt(x,y):
    global Ki,Kp,halfT,matrix
    global s1,s2,s3
    global NormAccz,u
    q0=1;q1=0;q2=0;q3=0;exInt=0;eyInt=0;ezInt=0
    matrix.clear()
    ax=x[0];ay=x[1];az=x[2]
    gx=y[0];gy=y[1];gz=y[2]
    norm=math.sqrt(ax*ax+ay*ay+az*az)
    ax = ax * norm;ay = ay * norm;az = az * norm
    vx = 2 * ((q1*q3) - (q0*q2))
    vy = 2 * ((q0*q1) + (q2*q3))
    vz = (q0*q0) - (q1*q1) - (q2*q2) + (q3*q3)
    ex = ay * vz - az * vy
    ey = az * vx - ax * vz
    ez = ax * vy - ay * vx
    
    exInt = exInt + ex * Ki
    eyInt = eyInt + ey * Ki
    ezInt = ezInt + ez * Ki

    gx = gx + Kp * ex + exInt
    gy = gy + Kp * ey + eyInt
    gz = gz + Kp * ez + ezInt

    q0 = q0 + ((-q1) * gx - q2 * gy - q3 * gz) * halfT
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT
    
    norm=math.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)
    q0 = q0 * norm;q1 = q1 * norm;q2 = q2 * norm;q3 = q3 * norm
    print(f"ex:{q1}")
    matrix.append( (q0*q0) + (q1*q1) - (q2*q2) - (q3*q3))
    matrix.append(2 * ((q1*q2) + (q0*q3)))
    matrix.append(2* ((q1*q3) - (q0*q2)))   
    matrix.append ( 2 * ((q1*q2) - (q0*q3)))	   
    matrix.append ((q0*q0) - (q1*q1) + (q2*q2) - (q3*q3))
    matrix.append (2* ((q2*q3) + (q0*q1))	)   
    matrix.append(2* ((q1*q3) + (q0*q2)) ) 
    matrix.append ( 2* ((q2*q3) - (q0*q1)))   
    matrix.append  ((q0*q0) - (q1*q1) - (q2*q2) + (q3*q3))
    vecxZ = -2 *(q1*q3)+2*(q0*q2)
    vecyZ = 2* (q2*q3) + 2 * (q0*q1)	 
    veczZ = (q0*q0) - (q1*q1) - (q2*q2) + (q3*q3)
    s1=y[2]*0.0610351*0.002
    #s1=math.atan2(2* ((q1*q2) + (q0*q3)), -2*((q2*q2)-(q3*q3)+1))* 57.295779#yaw
    print(f"vecxZ:{vecxZ}")
    try:
        s3=math.asin(vecxZ) * 57.295779 #pitch
    except:
        if vecxZ<-1:
            s3=-3.14/2* 57.295779
        if vecxZ>1:
            s3=3.14/2* 57.295779
    s2=math.atan2(vecyZ, -2 * q1 * q1 - 2 * q2* q2 + 1) * 57.295779#roll
    if  s2>90 or s2<-90:
        if s3>0:
            s3=180-s3
        if s3<0:
            s3=-(180+s3)
    u=[s1,s2,s3]
    NormAccz=(x[0]*vecxZ+x[1]*vecyZ+x[2]*veczZ)
    return u
def pj(z):
    i=s=0
    z.sort()
    for i in range(0,10):
        s+=z[i]
    s=s/10
    return s
def pian(s):
    s=int(s.hex(),16)
    if s>0x8000:
        s=-(0xffff-s)
    return s
while True:
    i2c.scan()
    for i in range(0,10):
        x=i2c.readfrom_mem(0x68,0x43, 1)#GX
        x+=i2c.readfrom_mem(0x68,0x44, 1)
        y=i2c.readfrom_mem(0x68,0x45, 1)
        y+=i2c.readfrom_mem(0x68,0x46, 1)
        z=i2c.readfrom_mem(0x68,0x47, 1)
        z+=i2c.readfrom_mem(0x68,0x48, 1)

        x1=i2c.readfrom_mem(0x68,0x3B, 1)#ax
        x1+=i2c.readfrom_mem(0x68,0x3C, 1)
        y1=i2c.readfrom_mem(0x68,0x3D, 1)
        y1+=i2c.readfrom_mem(0x68,0x3E, 1)
        z1=i2c.readfrom_mem(0x68,0x3F, 1)
        z1+=i2c.readfrom_mem(0x68,0x40, 1)
       
        z2=pian(z1)
        x2=pian(x1)
        y2=pian(y1)
        z22=pian(z)
        x22=pian(x)
        y22=pian(y)

        j1.append(z22)
        j2.append(x22)
        j3.append(y22)
        j4.append(z2)
        j5.append(x2)
        j6.append(y2)  
    x11=pj(j5)/32768*16*9.8
    print(x11)
    y11=pj(j6)/32768*16*9.8
    print(y11)
    z11=pj(j4)/32768*16*9.8
    print(z11)
    l=[x11,y11,z11]
    x12=pj(j2)/16.4*3.1415/180
    print(x12)
    y12=pj(j3)/16.4*3.1415/180
    print(y12)
    z12=pj(j1)/16.4*3.1415/180
    print(z12)
    k=[x12,y12,z12]
    print(k)
    print(l)
    print(f"zt:{zt(l,k)}")
    j1.clear()
    j2.clear()
    j3.clear()
    j4.clear()
    j5.clear()
    j6.clear()
    
'''
    while h<2000:
        x=s.i2c.readfrom_mem(0x68,0x43, 1)
        x+=s.i2c.readfrom_mem(0x68,0x44, 1)
        y=s.i2c.readfrom_mem(0x68,0x45, 1)
        y+=s.i2c.readfrom_mem(0x68,0x46, 1)
        z=s.i2c.readfrom_mem(0x68,0x47, 1)
        z+=s.i2c.readfrom_mem(0x68,0x48, 1)

        x2=s.i2c.readfrom_mem(0x68,0x3B, 1)
        x2+=s.i2c.readfrom_mem(0x68,0x3C, 1)
        y2=s.i2c.readfrom_mem(0x68,0x3D, 1)
        y2+=s.i2c.readfrom_mem(0x68,0x3E, 1)
        z2=s.i2c.readfrom_mem(0x68,0x3F, 1)
        z2+=s.i2c.readfrom_mem(0x68,0x40, 1)
    
        z3=int(z2.hex(),16)
        x3=int(x2.hex(),16)
        y3=int(y2.hex(),16)
        z1=int(z.hex(),16)
        x1=int(x.hex(),16)
        y1=int(y.hex(),16)
        z11+=z1
        x11+=x1
        y11+=y1
        z12+=z3
        x12+=x3
        y12+=y3
        h+=1
    gbx=x1-(x11/2000)
    gby=y1-(y11/2000)
    gbz=z1-(z11/2000)
    abx=x3-(x12/2000)
    aby=y3-(y12/2000)
    abz=z3-(z12/2000)
    if x1>0x8000:
        print(f"Gx:-{0xffff-x1-gbx}")
    else:
        print(f"Gx:+{x1-gbx}")
    if y1>0x8000:
        print(f"Gy:-{0xffff-y1-gby}")
    else:
        print(f"Gy:+{y1+gby}")
    if z1>0x8000:
        print(f"Gz:-{0xffff-z1-gby}")
    else:
        print(f"Gz:+{z1+gby}")
    if x3>0x8000:
        print(f"Ax:-{0xffff-x3-gby}")
    else:
        print(f"Ax:+{x3+gby}")
    if y3>0x8000:
        print(f"Ay:-{0xffff-y3-gby}")
    else:
        print(f"Ay:+{y3+gby}")
    if z3>0x8000:
        print(f"Az:-{0xffff-z3-gby}")
    else:
        print(f"Az:+{z3+gby}")
'''
#i2c.writeto(0x6A, b'\x0f')         # write 3 bytes to slave with 7-bit address 42i2c.readfrom(0x51, 4)             # read 4 bytes from slave with 7-bit address 42
     # write 1 byte to memory of slave 42,

#i2c.readfrom(0x6A,6)   
                                 # starting at address 2 in the slave
